function [desired_state] = diamond(t, qn)
% DIAMOND trajectory generator for a diamond

% =================== Your code goes here ===================
% You have to set the pos, vel, acc, yaw and yawdot variables

T = 12;
%  quintic trajectory 
if (t>=0) && (t<T/4)
pos = [(7.40148683083438e-17)*t-(5.08852219619863e-17)*t^2+0.0925925925925926*t^3-0.0462962962962963*t^4+0.00617283950617284*t^5;...
       (-2.96059473233375e-16)*t-(1.48029736616688e-16)*t^2+0.523782800878924*t^3-0.261891400439462*t^4+0.0349188533919283*t^5;...
       (-2.96059473233375e-16)*t-(1.48029736616688e-16)*t^2+0.523782800878924*t^3-0.261891400439462*t^4+0.0349188533919283*t^5];
vel = [(7.40148683083438e-17)-(5.08852219619863e-17)*2*t+0.0925925925925926*3*t^2-0.0462962962962963*4*t^3+0.00617283950617284*5*t^4;...
       (-2.96059473233375e-16)-(1.48029736616688e-16)*2*t+0.523782800878924*3*t^2-0.261891400439462*4*t^3+0.0349188533919283*5*t^4;... 
       (-2.96059473233375e-16)-(1.48029736616688e-16)*2*t+0.523782800878924*3*t^2-0.261891400439462*4*t^3+0.0349188533919283*5*t^4];
acc = [(-5.08852219619863e-17)*2+0.0925925925925926*6*t-0.0462962962962963*12*t^2+0.00617283950617284*20*t^3;...
       (-1.48029736616688e-16)*2+0.523782800878924*6*t-0.261891400439462*12*t^2+0.0349188533919283*20*t^3;...
       (-1.48029736616688e-16)*2+0.523782800878924*6*t-0.261891400439462*12*t^2+0.0349188533919283*20*t^3];

elseif (t>=T/4) && (t<T/2)
pos = [-7.50000000000003+10.0000000000000*t-5.00000000000000*t^2+1.20370370370370*t^3-0.138888888888889*t^4+0.00617283950617284*t^5;...
       45.2548339959390-56.5685424949238*t+28.2842712474619*t^2-6.80917641142601*t^3+0.785674201318386*t^4-0.0349188533919283*t^5;...
       -42.4264068711928+56.5685424949238*t-28.2842712474619*t^2+6.80917641142601*t^3-0.785674201318386*t^4+0.0349188533919283*t^5];
vel = [10.0000000000000-5.00000000000000*2*t+1.20370370370370*3*t^2-0.138888888888889*4*t^3+0.00617283950617284*5*t^4;...
       -56.5685424949238+28.2842712474619*2*t-6.80917641142601*3*t^2+0.785674201318386*4*t^3-0.0349188533919283*5*t^4;... 
       56.5685424949238-28.2842712474619*2*t+6.80917641142601*3*t^2-0.785674201318386*4*t^3+0.0349188533919283*5*t^4];
acc = [-5.00000000000000*2+1.20370370370370*6*t-0.138888888888889*12*t^2+0.00617283950617284*20*t^3;...
       28.2842712474619*2-6.80917641142601*6*t+0.785674201318386*12*t^2-0.0349188533919283*20*t^3;...
       -28.2842712474619*2+6.80917641142601*6*t-0.785674201318386*12*t^2+0.0349188533919283*20*t^3];

elseif (t>=T/2) && (t<T*3/4)
pos = [-127.500000000000+90.0000000000000*t-25*t^2+3.42592592592593*t^3-0.231481481481481*t^4+0.00617283950617284*t^5;...
       724.077343935027-509.116882454315*t+141.421356237310*t^2-19.3799636325202*t^3+1.30945700219731*t^4-0.0349188533919283*t^5;...
       726.905771059773-509.116882454315*t+141.421356237310*t^2-19.3799636325202*t^3+1.30945700219731*t^4-0.0349188533919283*t^5];
vel = [90.0000000000000-25*2*t+3.42592592592593*3*t^2-0.231481481481481*4*t^3+0.00617283950617284*5*t^4;...
       -509.116882454315+141.421356237310*2*t-19.3799636325202*3*t^2+1.30945700219731*4*t^3-0.0349188533919283*5*t^4;... 
       -509.116882454315+141.421356237310*2*t-19.3799636325202*3*t^2+1.30945700219731*4*t^3-0.0349188533919283*5*t^4];
acc = [-25*2+3.42592592592593*6*t-0.231481481481481*12*t^2+0.00617283950617284*20*t^3;...
       141.421356237310*2-19.3799636325202*6*t+1.30945700219731*12*t^2-0.0349188533919283*20*t^3;...
       141.421356237310*2-19.3799636325202*6*t+1.30945700219731*12*t^2-0.0349188533919283*20*t^3];

elseif (t>=T*3/4) && (t<T)
pos = [-735.000000000000+360.000000000000*t-70.0000000000000*t^2+6.75925925925926*t^3-0.324074074074074*t^4+0.00617283950617284*t^5;...
       -4163.44472762640+2036.46752981726*t-395.979797464467*t^2+38.2361444641615*t^3-1.83323980307623*t^4+0.0349188533919283*t^5;...
       4163.44472762640-2036.46752981726*t+395.979797464467*t^2-38.2361444641615*t^3+1.83323980307623*t^4-0.0349188533919283*t^5];
vel = [360.000000000000-70.0000000000000*2*t+6.75925925925926*3*t^2-0.324074074074074*4*t^3+0.00617283950617284*5*t^4;...
       2036.46752981726-395.979797464467*2*t+38.2361444641615*3*t^2-1.83323980307623*4*t^3+0.0349188533919283*5*t^4;... 
       -2036.46752981726+395.979797464467*2*t-38.2361444641615*3*t^2+1.83323980307623*4*t^3-0.0349188533919283*5*t^4];
acc = [-70.0000000000000*2+6.75925925925926*6*t-0.324074074074074*12*t^2+0.00617283950617284*20*t^3;...
       -395.979797464467*2+38.2361444641615*6*t-1.83323980307623*12*t^2+0.0349188533919283*20*t^3;...
       395.979797464467*2-38.2361444641615*6*t+1.83323980307623*12*t^2-0.0349188533919283*20*t^3];


else
     pos = [1; 0; 0];
     vel = [0; 0; 0];
     acc = [0; 0; 0];
end
%leave as 0
yaw = 0;
yawdot = 0;

% =================== Your code ends here ===================

desired_state.pos = pos(:);
desired_state.vel = vel(:);
desired_state.acc = acc(:);
desired_state.yaw = yaw;
desired_state.yawdot = yawdot;

end
